#include "common.h"

#ifdef PROBE

void timer0Overflow()
{
	Timer0Reg0++;			// increment low-order counter
	//debug("timer 0 overflow\n");
	adc_stream();
	LED2_PORT ^= _BV(LED2_PIN);              

	if (Timer0Reg0 == 50)
	{
		Timer0Reg0 = 0;
			
		//LEDPOST2();

		//timer0Overflow();
	}
}
void timer1Overflow(void)
{
	Timer1Reg0++;			// increment low-order counter
	//printf("timer 1 overflow\n");
	//sbi(LEDRED_PORT, LEDRED_PIN); //red
//	LEDPOST2();
	//_delay_ms(500);
	/*
	if(LEDRED_PIN | BV(LEDRED_PIN))
	{
		LEDRED_PORT &= ~(LEDRED_PIN);
	}*/
	//LEDRED_PORT ^= BV(LEDRED_PIN);
	//LEDIR_PORT ^= BV(LEDIR_PIN);
	
	if (Timer1Reg0 == 50)
	{
		Timer1Reg0 = 0;
		//timer1Overflow();
	}
}

void init_mcu(void)
{
	//set initial values to OFF
	
	//status LEDs are active LOW
	sbi(LED_PORT, LED_PIN); //status LED
	
	sbi(LED2_PORT, LED_PIN); //status LED2
	
	//Finger LEDs are active LOW
	cbi(LEDRED_PORT, LEDRED_PIN); //red
	
	cbi(LEDIR_PORT, LEDIR_PIN); //infra
		
	//set port directions for LEDs as outputs
	sbi(LED_DDR, LED_PIN); //status LED
	
	sbi(LED2_DDR, LED_PIN); //status LED2
	
	sbi(LEDRED_DDR, LEDRED_PIN); //red
	
	sbi(LEDIR_DDR, LEDIR_PIN); //infra
	
	LEDPOST2();
    //LEDPOST();
	//buzzerPOST();
	
	init_rf();
	
	//initialize DACs
	init_i2c();
	dac_write(DACFILTER_ADDR, 128);
   // dac_write(DACLED_ADDR, 160);//red with robust AC amplifier, not enough gain
	dac_write(DACLED_ADDR, 120);//red with very high gain, samy's original circuit
	//dac_write(DACLED_ADDR, 25);

}

void init_rf(void)
{
	//PCINT12
	//PCMSK1 |= 0x10; 
	PCMSK1 |= _BV(PCINT12); //select the pin for change interrupt
	EIMSK |= _BV(PCIE1); //enable pin change interrupt on group
	
	_delay_ms(100); //give the rf time to power up
	//configure_receiver();
	configure_transmitter();
	
}
void lambdaprobe(void)
{
		u08 i, k;
		int avg_notch;
		u08 fdetect;
		u08 dcreach;
		u08 delta;

		error_handler();
		flag_handler();
				
	    //sbi(LEDIR_PORT, LEDIR_PIN);			
		sbi(LEDRED_PORT, LEDRED_PIN); //red
		#define goal 3
		#define tollerance 50
		
		#define mid 128
		
		fdetect = 0;
		i = 1;
		dcreach = 0;

		while(1)
		{
				
				//for(i = 0; i < 255; i++)
				{
		    		dac_write(DACLED_ADDR, i);
					_delay_ms(1);
					avgavg = avg(a2dConvert8bit(ADC_NOTCH));
					/*
										temp = error;
					
					if(temp > 999) temp = 0;
					fpga_packet[1] = temp / 100;
					fpga_packet[2] = (temp % 100) / 10;
					fpga_packet[3] = (temp % 10);
					packet_send_FPGA(PACKET_SAO2);
					*/
					//_delay_ms(1);
					//avgavg += a2dConvert8bit(ADC_NOTCH);
					//_delay_ms(1);
					//avgavg += a2dConvert8bit(ADC_NOTCH);
					//avgavg/=3;
					//sudden insertion of finger
					/*
					if(avgavg >= 255-(delta*20)) {
						i = 0;
						dac_write(DACLED_ADDR, i);
						_delay_ms(500);
					}*/
					
				   if (dcreach == 0)
					   delta = goal;
				   else
					   delta = tollerance;

				   if (fdetect == 1){
						   if(avgavg < (mid - delta)){
								   i--;
								   if (i == 0){
										   i = 1;
										   fdetect = 0;
										   dcreach = 0;
								   }
						   }else if(avgavg > (mid + delta))
								   i++;
								else
									dcreach = 1;
				   } else{
						   if (avgavg > 40)
								   fdetect = 1;
				   }

					_delay_ms(15) ;
					//_delay_ms(200);
					//_delay_ms(200);
					//_delay_ms(200);_delay_ms(200);
					//_delay_ms(200);_delay_ms(200);
					/*_delay_ms(200);_delay_ms(200);
					_delay_ms(200);
					_delay_ms(200);_delay_ms(200);
					_delay_ms(200);
					*/
					
				}
		}
		//test function
		//streamADC(); //blocking streaming
		
		//bootup sequence for probe
		//init ports, LEDs off, DACs at zero volts
		//check battery level
			//low battery detected
				//flag to send low battery alarm to lambda
				//set 'low batt' flag/mode
				//continue to RF check
		//check RF interlink
			//no RF-> "independent" mode: beacon periodically for RF interlink
			//RF found->send initial configuration->
				//RF stability check
					//RF unstable->switch channels
					//RF stable->"normal" mode
		//check optical sensor
			//attempt auto calibration (sweep and find midpoint)
			//obstruction detected (nominal voltage@ nominal distance)
				//detect object type->sweep LEDs
					//translucent
						//detect a pulse
							//no pulse->errorA
							//pulse
								//RF: send waveform data
								//goto SaO2 sampling
					//opaque->error: sensor failure/remove obstruction
			//obstruction not detected
				//send idle command to lambda board
				//goto optical check
				
		//object identification
			//single LED intensity sweep
			//finger detected
				//begin finger sampling, assumes rate of change of O2 small compared to aquisition time
					//ramp DAC for IR LED
					//read A/D until desired level
					//swtich LEDs
					//ramp DAC for RED LED
					//read A/D until desired level 
					//basic RMS calculation
					//RF: send calculation to lambda

	return;
}
void init_i2c(void)
{
	USI_TWI_Master_Initialise();
}
void dac_write(u08 addr, u08 voltage)
{
	static u08 current_addr = 0;
	u08 messageBuf[MESSAGEBUF_SIZE];
	u08 result;
	//write analog value to DAC
	  //Ctrl/MS-Byte:
	  //0 0 PD1 PD0 D7 D6 D5 D4 
	  //D3 D2 D1 D0 X X X X
	  //PD0, PD1: 0 0 normal operation
	  //1 0 0 1 1 0 A0 R/W
	messageBuf[1] = 0x00 | (voltage >> 4);             // The first byte is used for commands.
	messageBuf[2] = 0x00 | (voltage <<4);                        // The second byte is used for the data.
	
	if(1)//current_addr!=addr) //not a consecutive update, 
	{
		//current_addr = addr;
		messageBuf[0] = (addr) | (FALSE<<TWI_READ_BIT); // The first byte must always consit of General Call code or the TWI slave address.

		result = USI_TWI_Start_Transceiver_With_Data( messageBuf, 3 ); 
#warning "reminder: add error returns from transceiver"		
	} else { //consecutive update, just send the voltage
		//messageBuf[0] = (addr | 2) | (FALSE<<TWI_READ_BIT); // The first byte must always consit of General Call code or the TWI slave address.
		result = USI_TWI_Start_Transceiver_With_Data(&messageBuf[1], 2); 
	}
	
	// _delay_us(100);
	// _delay_us(50);
	//_delay_us(25);
	// _delay_us(1); //ok
	//__builtin_avr_delay_cycles(1); //ok gives 41Hz triangle; 41Hz*256 = 10496 samples/sec
}

void LEDPOST2(void)
{
    printf("LED2 POST\n");
	sbi(LED2_DDR, LED2_PIN);
	              
    cbi(LED2_PORT, LED2_PIN);              
	_delay_ms(250); 
	sbi(LED2_PORT, LED2_PIN);
	_delay_ms(250);
	cbi(LED2_PORT, LED2_PIN);
}
void adc_stream(void)
{
	static u08 count=0;
	count++;
	/*		
	rf_packet[0] = RFPACKET_ADCSTREAMH;
	rf_packet[1] = count;
	rf_packet[2] = a2dConvert8bit(4); //raw sensor
	rf_packet[3] = a2dConvert8bit(3); //LPF */
	//transmit_data();
	//packet_send_rf();
	rf_packet[0] = RFPACKET_ADCSTREAML;
	rf_packet[1] = a2dConvert8bit(2); //AC amp
	rf_packet[2] = a2dConvert8bit(1); //notch
	rf_packet[3] = a2dConvert8bit(0); //battery
	transmit_data();
	#warning "packet_send_rf slows down interface significantly!!!!"
	//packet_send_rf();
}
#endif
